#include <SPI.h>
#include <inttypes.h>
#include <Arduino.h>

#include "global.h"
#include "extEEPROM.h"
#include "WARDEN.h"
#include "GPS.h"


//TESTING
#define LED_DEBUG
#define SERIAL_DEBUG

#ifdef LED_DEBUG
	int toggles[8]; 
	void toggle(int);
	void byteLite(uint8_t);
	volatile uint8_t numSats=0;
	volatile uint8_t packetCount=0;
#endif

#ifdef SERIAL_DEBUG
	//#define TEST_WRITE
	#define TEST_READ
	//#define WR_VERBOSE
	//#define RD_VERBOSE
#endif


//objects
//GPS gps = GPS();
//Warden warden = Warden(Coordinate(42.393634, -72.529214));
//extern extEEPROM spiPROM;

//RX call-back function
int busStation();

//buffer for reading Serial data
volatile uint8_t buffGPS[MAX_GPS_PKTLEN];
volatile bool flgNewGPS=false;


//for logging
Coordinate GPSrecord[GPS_PAGE_LEN];
volatile int numRecs;

void setup()
{
	#ifdef SERIAL_DEBUG
		Serial.begin(9600);
	#endif

	#ifndef SERIAL_DEBUG
		Serial.beginWithInterrupt(38400, busStation);
	#endif

	pinMode(2, OUTPUT);
	pinMode(3, OUTPUT);
	pinMode(4, OUTPUT);
	pinMode(5, OUTPUT);
	pinMode(6, OUTPUT);
	pinMode(7, OUTPUT);
	pinMode(8, OUTPUT);
	pinMode(9, OUTPUT);
	pinMode(10, OUTPUT);
	pinMode(11, OUTPUT);
	pinMode(12, INPUT);
	pinMode(13, OUTPUT);
	pinMode(A0, OUTPUT);

	spiPROM.init();

	numRecs=0;
}

int numRuns=0;

void loop()
{

#ifndef SERIAL_DEBUG
	byteLite(numSats+1);

	if(flgNewGPS){ //&& numSats>0x03){
		
		//byteLite(packetCount++);
		GPSrecord[numRecs] = gps.process(buffGPS);
		numRecs++;
		if(numRecs>9){
			
			spiPROM.writeCoordPack(0, GPSrecord);
			byteLite(0x0f);
			delay(300);
			byteLite(0xf0);
			delay(300);
			byteLite(0x0f);
			delay(300);
			byteLite(0xff);
			delay(10000);
			
		}
		flgNewGPS=false;
		delay(1000);
		Serial.beginWithInterrupt(38400, busStation);
	}

#endif

#ifdef TEST_WRITE
		Serial.print("write ");
		Coordinate tstCoordPack[10];
		unsigned long tmStp;
		for(int i=0; i<10; i++){
			tmStp=millis();
			tstCoordPack[i].lat=-78+numRuns+i;
			tstCoordPack[i].lon=12+numRuns+i;
			tstCoordPack[i].timeStamp=millis(); //micros(); i thought it wasn't working, turns out computers are fast
		}

		spiPROM.writeCoordPack(0, tstCoordPack);
		while(spiPROM.readStatus()&0x01==1){
			Serial.print("|");
		}
		Serial.println();
#endif


#ifdef TEST_READ
		Serial.print("read ");
		for(int i=0; i<10; i++){
			Coordinate CoordReadTest;

			CoordReadTest = spiPROM.readCoord(i*COORD_STORE_LEN);
			delay(2000);
			Serial.print("lat: ");
			Serial.println(CoordReadTest.lat);
			
			Serial.print("lon: ");
			Serial.println(CoordReadTest.lon);
			
			Serial.print("time: ");
			Serial.println(CoordReadTest.timeStamp);
			Serial.println();
		}
		
#endif

numRuns++;
}

		//for(i=0; i<16; i++){
		//	CoordWriteTest.lat = -300+i;
		//	CoordWriteTest.lon = 255-i;
		//	CoordWriteTest.timeStamp = millis();
		//	spiPROM.writeCoord(i*COORD_STORE_LEN, CoordWriteTest);
		//	delay(1000);
		//}

		//uint8_t* ptrTest;
		//ptrTest = (uint8_t*)(&CoordWriteTest.lat);
	
		//Serial.println(*ptrTest++);
		//Serial.println(*ptrTest++);
		//Serial.println(*ptrTest++);
		//Serial.println(*ptrTest++);	


int busStation(){     //called with each new byte in
	static volatile uint8_t prevByte=0, newByte=0;
	static bool flgInGPSPkt=false;
	static int nBytesRXd=0;
	if(!flgNewGPS){
		newByte = Serial.read();  //read a byte every time this function is called, always keep tract of byteIn and prevByte
			if(flgInGPSPkt){
				if(nBytesRXd>MAX_GPS_PKTLEN){   //some sort of fail
					nBytesRXd=0;
					flgInGPSPkt=false;
				}

				else{							//not fail
					if(prevByte==END1 && newByte==END2){
						nBytesRXd=0;
						Serial.begin(9600);
						flgNewGPS=true;       //inner most case, success
					}
					else
						buffGPS[nBytesRXd]=newByte;
				}
			}
				
			if(prevByte==START1 && newByte==START2){  //if you're not working on a packet already, and you see the start of one, go into it
				nBytesRXd=0;
				flgInGPSPkt=true;
			} 

		prevByte=newByte;
	}
	return 0;
}

void toggle(int bit){
	if(!toggles[bit]){
		digitalWrite((BYTE_LED_LSB+bit), HIGH);
		toggles[bit]=1;
	}

	else{
		digitalWrite((BYTE_LED_LSB+bit), LOW);
		toggles[bit]=0;
	}
}

void byteLite(uint8_t byteQ){
	for(int i=0; i<8; i++){
		if(((byteQ>>i) & 0x01)==0x01)
			digitalWrite(BYTE_LED_LSB+i, HIGH);
		else digitalWrite(BYTE_LED_LSB+i, LOW);
	}
}
